#include "common/path.h"

inline float normalize_angle(float rad) {
    rad = std::fmod(rad + M_PI, 2 * M_PI);        // 先偏移 +π，取模
    return (rad <= 0) ? rad + M_PI : rad - M_PI;  // 再偏移回 [-π, π]
}

CubicPolynomial::CubicPolynomial(const PathPoint& start_path_point, const PathPoint& end_path_point)
    : start_path_point_(start_path_point), end_path_point_(end_path_point) {
    Eigen::Vector4f temp_coeffs = compute_cubic_polynomial_coeffs(start_path_point, end_path_point);
    coeffs_.start_s = start_path_point.s;
    coeffs_.end_s = end_path_point.s;
    coeffs_.a0 = temp_coeffs[0];
    coeffs_.a1 = temp_coeffs[1];
    coeffs_.a2 = temp_coeffs[2];
    coeffs_.a3 = temp_coeffs[3];
}

Eigen::Vector4f CubicPolynomial::compute_cubic_polynomial_coeffs(const PathPoint& p1, const PathPoint& p2) {
    float s1 = p1.s, l1 = p1.l, tan1 = tan(p1.theta);
    float s2 = p2.s, l2 = p2.l, tan2 = tan(p2.theta);

    Eigen::Matrix4f A;
    A << 1, s1, s1 * s1, s1 * s1 * s1, 0, 1, 2 * s1, 3 * s1 * s1, 1, s2, s2 * s2, s2 * s2 * s2, 0, 1, 2 * s2, 3 * s2 * s2;

    Eigen::Vector4f b;
    b << l1, tan1, l2, tan2;

    Eigen::Vector4f coeffs = A.colPivHouseholderQr().solve(b);
    return coeffs;
}

Path::Path(int path_index, float segment_length, const std::vector<RefPoint>* refline, const std::vector<PathPoint>& key_path_points)
    : path_index_(path_index), segment_length_(segment_length), refline_(refline) {
    assert(key_path_points.size() > 1);

    min_s_ = key_path_points.front().s;
    max_s_ = key_path_points.back().s;
    std::vector<CubicPolynomial> tmp_cubic_polynomial_vec;
    for (int i = 0; i < key_path_points.size() - 1; i++) {
        tmp_cubic_polynomial_vec.emplace_back(key_path_points[i], key_path_points[i + 1]);
    }
    cubic_polynomial_vec_ = tmp_cubic_polynomial_vec;
}

float Path::get_l_by_s(float s) const {
    auto it = std::lower_bound(cubic_polynomial_vec_.begin(), cubic_polynomial_vec_.end(), s,
                               [](const CubicPolynomial& polynomial, float s) { return polynomial.max_s() < s; });

    if (it == cubic_polynomial_vec_.end()) {
        it--;
    }

    return it->get_l_by_s(s);
}

void Path::get_point_info_by_s(float s, PointInfo* point_info) const {
    float l = get_l_by_s(s);
    point_info->path_index = path_index_;
    point_info->point_index = static_cast<int>((s - min_s_ + 1e-6) / kPointInfoResolution);
    point_info->s = s;
    point_info->l = l;

    float s0 = refline_->front().s;
    int ref_index = static_cast<int>((s - s0 + 1e-6) / kRefPointResolution);
    const auto& ref_point = refline_->at(ref_index);
    point_info->theta_ref = ref_point.theta;

    float angle_dir = l > 0 ? ref_point.theta + M_PI / 2 : ref_point.theta - M_PI / 2;
    angle_dir = normalize_angle(angle_dir);
    point_info->x = ref_point.x + std::abs(l) * std::cos(angle_dir);
    point_info->y = ref_point.y + std::abs(l) * std::sin(angle_dir);
}

int Path::serialize_path_info(PathInfo* buffer) const {  // 将path的信息序列化, 方便GPU使用
    assert(IsAligned(buffer, kAligned));
    PathInfo* path_info = new (buffer) PathInfo();
    path_info->path_index = path_index_;
    path_info->min_s = min_s_;
    path_info->max_s = max_s_;
    path_info->cubic_polynomial_coeff_nums = cubic_polynomial_vec_.size();
    return sizeof(PathInfo);
}

int Path::serialize_path_coeffs(CubicPolynomialCoeffs* buffer) const {
    assert(IsAligned(buffer, kAligned));
    for (int i = 0; i < cubic_polynomial_vec_.size(); i++) {
        auto& cubic_polynomial = cubic_polynomial_vec_[i];
        CubicPolynomialCoeffs* coeffs = buffer + i;
        *coeffs = cubic_polynomial.coeffs();
    }
    return cubic_polynomial_vec_.size();
}

void Path::sampling_on_cpu(float delat_s) const {
    PointInfo point_info;
    float s = min_s_;
    while (s <= max_s_) {
        get_point_info_by_s(s, &point_info);
        s += delat_s;
    }
}

void Path::dump(std::ofstream& out_file, float delat_s) const {
    out_file << path_index_ << ", ";

    PointInfo point_info;
    float s = min_s_;
    while (s <= max_s_) {
        get_point_info_by_s(s, &point_info);
        out_file << "(";
        out_file << point_info.x << ", " << point_info.y;
        out_file << "), ";
        s += delat_s;
    }
    out_file << std::endl;
}